#include "RobotController.h"
#include <QJsonDocument>
#include <QJsonParseError>
#include <QJsonObject>
#include <QCoreApplication>
#include <QHash>
#include <QDir>
#include "SCHeadData.h"
#include "../global_logger.h"

const QHash<int, QString> RobotController::stateMap = {
    {0, "NONE"},
    {2, "RUNNING"},
    {3, "SUSPENDED"},
    {4, "COMPLETED"},
    {5, "FAILED"},
    {6, "CANCELED"}
};

RobotController::RobotController(QObject *parent) : QObject(parent) {
    m_worker = new StatusWorker(15000);   // 15秒间隔
    m_worker->moveToThread(&m_workerThread);

    // 连接工作线程信号到主线程槽函数
    connect(m_worker, &StatusWorker::requestBattery, this, &RobotController::getRobotBattery);
    connect(m_worker, &StatusWorker::requestPosition, this, &RobotController::getRobotPosition);
    connect(m_worker, &StatusWorker::requestNavState, this, &RobotController::getRobotNavigationalState);
    
    m_workerThread.start(); // 内部自动调用 exec() 
    LOG_INFO("RobotController 初始化完成,状态轮询工作线程已启动.");
}

void RobotController::connectRobot(const QString &ip) {
    LOG_INFO("尝试连接机器人控制器 - IP: {}", ip.toStdString());
    m_ctrlSocket.connectHost(ip, SEER_API_PORT_CTRL);
    m_naviSocket.connectHost(ip, SEER_API_PORT_NAVI);
    m_statSocket.connectHost(ip, SEER_API_PORT_STAT);
    m_pushSocket.connectHost(ip, SEER_API_PORT_PUSH);
    m_confSocket.connectHost(ip, SEER_API_PORT_CONF);
    

    // 使用内部 socket 连接信号
    connect(m_statSocket.tcpSocket(), &QTcpSocket::connected, [this]() {
        LOG_INFO("SRC控制器连接成功");
        // 连接成功后自动获取当前地图
        getCurrentMap();
    });
    
    // 添加断开连接的处理
    connect(m_statSocket.tcpSocket(), &QTcpSocket::disconnected, [this]() {
        LOG_INFO("机器人控制器断开连接");
        // 停止状态轮询
        if (m_worker) {
            m_worker->stop();
            LOG_DEBUG("状态轮询已停止");
        }
    });
    
    connect(m_statSocket.tcpSocket(), QOverload<QAbstractSocket::SocketError>::of(&QAbstractSocket::errorOccurred),
        [this](QAbstractSocket::SocketError error) {
            LOG_ERROR("机器人控制器Socket错误: {}", static_cast<int>(error));
            // 停止状态轮询
            if (m_worker) {
                m_worker->stop();
            }
        });

    // 电池、位置、导航状态轮询定时器
    m_worker->start();
    LOG_DEBUG("启动电池、位置、导航状态轮询定时器");
}

RobotController::~RobotController() {
    // 在线程退出前停止定时器
    if (m_worker) {
        m_worker->stop();
        LOG_DEBUG("状态轮询已停止");
    }
    
    // 断开所有连接
    disconnect(m_worker, nullptr, this, nullptr);
    
    // 退出并等待线程结束
    m_workerThread.quit();
    m_workerThread.wait();
    LOG_DEBUG("工作线程已退出");
    // 删除worker对象
    if (m_worker) {
        m_worker->deleteLater(); // 改为 deleteLater 确保线程安全
        m_worker = nullptr;
    }
}

void RobotController::disconnectRobot() {
    // 停止定时器
    if (m_worker) {
        m_worker->stop();
    }
    
    m_ctrlSocket.releaseTcpSocket();
    m_naviSocket.releaseTcpSocket();
    m_statSocket.releaseTcpSocket();
    m_pushSocket.releaseTcpSocket();
    m_confSocket.releaseTcpSocket();
    
    QMutexLocker locker(&m_dataMutex);
    m_battery = -1.0;
    m_position = PositionData{}; // 结构体清零
    m_navState.clear();
    m_currentTarget.clear();
    emit batteryUpdated(-1); // 通知UI重置状态
    LOG_INFO("机器人控制器盒子断开成功");
}

bool RobotController::isConnected() const {
    bool connected = m_statSocket.tcpSocket()->state() == QAbstractSocket::ConnectedState;
    LOG_DEBUG("检查连接状态: {}", connected ? "已连接" : "未连接");
    return connected;
}

void RobotController::relocate(float x, float y, float z) {
    LOG_INFO("执行重定位操作 - 位置: ({}, {}, {})", x, y, z);
    QJsonObject obj;
    obj["x"] = x;
    obj["y"] = y;
    obj["z"] = z;
    QJsonDocument doc(obj);
    QByteArray json = doc.toJson(QJsonDocument::Compact);

    m_requestManager.sendRequest(&m_naviSocket, CMD_CTRL_RELOC, json, 
        [this](const QByteArray &resp) {
            bool success = !resp.isEmpty();
            emit operationCompleted(CMD_CTRL_RELOC, success);
            LOG_DEBUG("重定位操作完成 - 结果: {}", success ? "成功" : "失败");
        }
    );
}

void RobotController::runGoal(const QString &start, const QString &end)
{
    LOG_INFO("执行路径规划任务 - 起点: {}, 终点: {}", 
                       start.toStdString(), end.toStdString());
        
    // TODO: 实际发送路径规划指令
    Q_UNUSED(start);
    Q_UNUSED(end);
}

/**
 * @brief 获取机器人电池信息
 * @param cmdId 命令ID，用于标识本次请求
 * 
 * 该函数通过发送请求获取机器人的电池状态信息。
 * 首先检查与机器人的连接状态，如果未连接则直接返回。
 * 通过请求管理器发送获取电池信息的命令，并通过回调函数处理响应结果。
 */
void RobotController::getRobotBattery(int cmdId){
    // 检查连接状态
    if (!isConnected()) {
        return;
    }
    
    // 发送获取电池信息请求，并设置回调函数处理响应
    m_requestManager.sendRequest(&m_statSocket, cmdId, QByteArray(),
        [this, cmdId](const QByteArray &resp) {
            bool success = !resp.isEmpty();
            handleOperationCompleted(cmdId, success, resp);
        }
    );
}

/**
 * @brief 获取机器人位置信息
 * @param cmdId 命令ID，用于标识本次请求
 * 
 * 该函数通过发送请求获取机器人的当前位置信息。
 * 首先检查与机器人的连接状态，如果未连接则直接返回。
 * 通过请求管理器发送获取位置信息的命令，并通过回调函数处理响应结果。
 */
void RobotController::getRobotPosition(int cmdId){
    // 检查连接状态
    if (!isConnected()) {
        return;
    }
    
    // 发送获取位置信息请求，并设置回调函数处理响应
    m_requestManager.sendRequest(&m_statSocket, cmdId, QByteArray(),
        [this, cmdId](const QByteArray &resp) {
            bool success = !resp.isEmpty();
            handleOperationCompleted(cmdId, success, resp);
        }
    );
}

/**
 * @brief 获取机器人导航状态信息
 * @param cmdId 命令ID，用于标识本次请求
 * 
 * 该函数通过发送请求获取机器人的导航状态信息。
 * 首先检查与机器人的连接状态，如果未连接则直接返回。
 * 通过请求管理器发送获取导航状态信息的命令，并通过回调函数处理响应结果。
 */
void RobotController::getRobotNavigationalState(int cmdId){
    // 检查连接状态
    if (!isConnected()) {
        return;
    }
    
    // 发送获取导航状态信息请求，并设置回调函数处理响应
    m_requestManager.sendRequest(&m_statSocket, cmdId, QByteArray(),
        [this, cmdId](const QByteArray &resp) {
            bool success = !resp.isEmpty();
            handleOperationCompleted(cmdId, success, resp);
        }
    );
}

/**
 * @brief 处理机器人操作完成的回调函数
 * 
 * 该函数用于处理来自机器人设备的操作完成事件。根据命令ID解析返回的数据，
 * 更新内部状态并发出相应的信号通知外部观察者。
 * 
 * @param cmdId 命令ID，标识完成的操作类型
 * @param success 操作是否成功执行的标志
 * @param data 返回的数据内容，通常为JSON格式
 */
void RobotController::handleOperationCompleted(int cmdId, bool success, const QByteArray& data) {
    LOG_DEBUG("处理操作完成回调 - 命令ID: {}, 成功: {}", cmdId, success);
    
    if (!success) {
        LOG_WARN("操作失败 - 命令ID: {}", cmdId);
        return;
    }

    QMutexLocker locker(&m_dataMutex); // 加锁保护共享数据
    
    // 解析并缓存数据
    QJsonParseError parseError;
    QJsonDocument doc = QJsonDocument::fromJson(data, &parseError);
    if (parseError.error != QJsonParseError::NoError) {
        LOG_ERROR("JSON解析错误: {}", parseError.errorString().toStdString());
        return;
    }
    locker.unlock();
    
    switch (cmdId) {
        case CMD_STATE_GET_BATTERY: {
            double newBattery = doc.object()["battery_level"].toDouble();
            //LOG_INFO("电池状态更新 - 电量: {}%", newBattery);
            if (!qFuzzyCompare(newBattery, m_battery)) {
                m_battery = newBattery;
                emit batteryUpdated(newBattery); // 独立信号
                LOG_DEBUG("电池电量更新 - 新电量: {}%", newBattery);
            }
            break;
        }
        case CMD_STATE_GET_POSITION: {
            QJsonObject posObj = doc.object();
            double x = posObj["x"].toDouble();
            double y = posObj["y"].toDouble();
            double angle = posObj["angle"].toDouble();
            //LOG_INFO("位置更新 - 坐标: ({}, {}), 角度: {}", x, y, angle);
            if (!qFuzzyCompare(x, m_position.x) || 
                !qFuzzyCompare(y, m_position.y)) {
                m_position = {x, y, angle};
                emit positionUpdated(x, y, angle); // 独立信号
                LOG_DEBUG("机器人位置更新 - 新位置: ({}, {}), 角度: {}", x, y, angle);
            }
            break;
        }
        case CMD_STATE_GET_NAVIGATIONAL_STATE: {
            QJsonObject jsonObj = doc.object();
            int taskStatus = jsonObj["task_status"].toInt();
            QString newState = stateMap.value(taskStatus, "UNKNOWN");
            QString newTarget = jsonObj["target_id"].toString();
            
            //LOG_INFO("导航状态更新 - 状态: {}, 目标: {}", newState.toStdString(), newTarget.toStdString());

            // 直接更新状态并上报，不进行判断
            m_navState = newState;
            m_currentTarget = newTarget;
            emit navigationStateUpdated(newState, newTarget);
            LOG_DEBUG("导航状态更新完成 - 状态: {}, 目标: {}", 
                                newState.toStdString(), newTarget.toStdString());
            break;
        }
        default: {
            LOG_WARN("未知命令ID: {}", cmdId);
            break;
        }
    }
}

/**
 * @brief 设置机器人控制器的参数
 * @param robotId 机器人唯一标识符
 * @return 无返回值
 */
void RobotController::setParameters(const QString robotId){
    m_robotId = robotId;
    LOG_DEBUG("设置机器人参数 - 机器人ID: {}", robotId.toStdString());
}

/**
 * @brief 获取当前加载的地图名称
 */
void RobotController::getCurrentMap() {    
    // 检查连接状态
    if (!isConnected()) {
        LOG_WARN("获取当前地图失败: 机器人未连接");
        return;
    }
        
    // 发送获取当前地图请求
    m_requestManager.sendRequest(&m_statSocket, CMD_MAP_GET_CURRENT, QByteArray(),
        [this](const QByteArray &resp) {
            if (resp.isEmpty()) {
                LOG_WARN("获取当前地图失败: 响应数据为空");
                return;
            }
            
            QJsonParseError parseError;
            QJsonDocument doc = QJsonDocument::fromJson(resp, &parseError);
            if (parseError.error != QJsonParseError::NoError) {
                LOG_ERROR("解析当前地图响应失败: {}", parseError.errorString().toStdString());
                return;
            }

            QString currentMap = doc.object()["current_map"].toString();
            LOG_DEBUG("获取到当前地图名称: {}", currentMap.toStdString());
            
            if (!currentMap.isEmpty() && m_currentMapName != currentMap) {
                m_currentMapName = currentMap;
                LOG_INFO("当前地图更新为: {}", currentMap.toStdString());
            }
            
            if (!currentMap.isEmpty()) {
                // 检查本地是否存在该地图
                QString mapFileName = currentMap;
                if (!mapFileName.endsWith(".smap")) {
                    mapFileName += ".smap";
                }
                QString mapFilePath = QString("data/%1/map/%2").arg(m_robotId, mapFileName);
                QFile mapFile(mapFilePath);

                if (!mapFile.exists()) {
                    LOG_INFO("地图文件不存在，开始下载 - 地图: {}, 路径: {}", 
                                    currentMap.toStdString(), mapFilePath.toStdString());
                    downloadMap(currentMap);
                } else {
                    LOG_DEBUG("地图文件已存在 - 地图: {}", currentMap.toStdString());
                }
            } else {
                LOG_WARN("获取到的当前地图名称为空");
            }
        }
    );
}

/**
 * @brief 下载指定地图
 * @param mapName 地图名称
 */
void RobotController::downloadMap(const QString& mapName) {
    LOG_INFO("开始下载地图 - 地图名称: {}", mapName.toStdString());
    
    // 创建地图目录
    QString mapDirPath = QString("data/%1/map").arg(m_robotId);
    QDir mapDir(mapDirPath);
    if (!mapDir.exists()) {
        if (mapDir.mkpath(".")) {
            LOG_DEBUG("创建地图目录成功 - 路径: {}", mapDirPath.toStdString());
        } else {
            LOG_WARN("创建地图目录失败 - 路径: {}", mapDirPath.toStdString());
        }
    }
        
    // 构造请求JSON
    QJsonObject obj;
    obj["map_name"] = mapName;
    QJsonDocument doc(obj);
    QByteArray jsonData = doc.toJson(QJsonDocument::Compact);

    // 发送下载地图请求
    m_requestManager.sendRequest(&m_confSocket, CMD_MAP_DOWNLOAD, jsonData,
        [this, mapName, mapDirPath](const QByteArray &resp) {
            if (resp.isEmpty()) {
                LOG_WARN("地图下载失败 - 地图名称: {}, 响应数据为空", mapName.toStdString());
                return;
            }

            // 保存地图数据到文件，添加.smap后缀
            QString mapFileName = mapName;
            if (!mapFileName.endsWith(".smap")) {
                mapFileName += ".smap";
            }
            QString mapFilePath = QString("%1/%2").arg(mapDirPath, mapFileName);
            QFile mapFile(mapFilePath);
            if (mapFile.open(QIODevice::WriteOnly)) {
                qint64 bytesWritten = mapFile.write(resp);
                mapFile.close();
                if (bytesWritten == resp.size()) {
                    LOG_INFO("地图下载成功 - 地图: {}, 文件路径: {}, 大小: {} 字节", 
                                       mapName.toStdString(), mapFilePath.toStdString(), bytesWritten);
                } else {
                    LOG_ERROR("地图文件写入不完整 - 地图: {}, 期望写入: {} 字节, 实际写入: {} 字节", 
                                        mapName.toStdString(), resp.size(), bytesWritten);
                }
            } else {
                LOG_ERROR("保存地图文件失败 - 文件路径: {}, 错误信息: {}", 
                                    mapFilePath.toStdString(), mapFile.errorString().toStdString());
            }
        }
    );
}